package com.jsurf;

//import edu.cicese.ans.Match;
//import org.cicese.ans.Label;
//
//import java.io.BufferedWriter;
//import java.io.File;
//import java.io.FileWriter;
//import java.io.FilenameFilter;
import java.util.ArrayList;
import java.util.List;

//import java.util.Collections;
//import java.util.HashMap;
//import java.util.Map;

public class Surf {
	
	private static final int HESSIAN_OCTAVES = 4;
	private static final int HESSIAN_INIT_SAMPLE = 2;
	public static float HESSIAN_THRESHOLD = 0.0001F;
	
	private List<InterestPoint> mFreeOrientedPoints;
	private List<InterestPoint> mUprightPoints;
	private List<InterestPoint> mDescriptorFreeInterestPoints;
	
	//octaves = steps
	private int mOctaves = HESSIAN_OCTAVES;
	private float mThreshold = HESSIAN_THRESHOLD;
	
	private IntegralImage mIntegralImage;
	
	public Surf(String imgFilename){
		mIntegralImage = new IntegralImage();
		mIntegralImage.getIntegralImage(SurfUtil.getIplImage(imgFilename));
	}
	
	public List<InterestPoint> getUprightInterestPoints() {
		return getPoints(true);
	}
	
	public List<InterestPoint> getFreeOrientedInterestPoints() {
		return getPoints(false);
	}
	
	private List<InterestPoint> getPoints(boolean upright) {
		List<InterestPoint> points = upright ? mFreeOrientedPoints : mUprightPoints;
		FastHessian mHessian = new FastHessian(mIntegralImage, mOctaves, HESSIAN_INIT_SAMPLE, mThreshold/*, mBalanceValue*/);
		points = mHessian.getIPoints();
		for (InterestPoint point : points){
			getOrientation(point);
			getMDescriptor(point, upright);
		}
		return points;
	}
	
	//! Assigns the supplied Ipoint an orientation
	private void getOrientation(InterestPoint point) {
		float gauss;
		float scale = point.getScale();
		
		int s = Math.round(scale);
		int r = Math.round(point.getY());
		int c = Math.round(point.getX());
		
		List<Float> xHaarResponses = new ArrayList<Float>();
		List<Float> yHaarResponses = new ArrayList<Float>();
		List<Float> angles = new ArrayList<Float>();
		
		// calculate haar responses for points within radius of 6*scale
		for (int i = -6; i <= 6; ++i) {
			for (int j = -6; j <= 6; ++j) {
				if (i * i + j * j < 36) {
					gauss = (float)SurfUtil.GAUSS25[Math.abs(i)][Math.abs(j)];
					float xHaarResponse = gauss * haarX(r + j * s, c + i * s, 4 * s);
					float yHaarResponse = gauss * haarY(r + j * s, c + i * s, 4 * s);
					xHaarResponses.add(xHaarResponse);
					yHaarResponses.add(yHaarResponse);
					angles.add(getAngle(xHaarResponse, yHaarResponse));
				}
			}
		}
		
		// calculate the dominant direction
		float sumX = 0, sumY = 0;
		float ang1, ang2, ang;
		float max = 0;
		float orientation = 0;
		
		// loop slides pi/3 window around feature point
		float pi = SurfUtil.PI;
		for (ang1 = 0; ang1 < 2 * pi;  ang1 += 0.15f) {
			ang2 = ang1 + pi / 3.0f > 2 * pi ? ang1 - 5.0f * pi / 3.0f : ang1 + pi / 3.0f;
			sumX = sumY = 0;
			for (int k = 0; k < angles.size(); k++) {
				// get angle from the x-axis of the sample point
				ang = angles.get(k).floatValue();
				
				// determine whether the point is within the window
				if (ang1 < ang2 && ang1 < ang && ang < ang2) {
					sumX += xHaarResponses.get(k).floatValue();
					sumY += yHaarResponses.get(k).floatValue();
				} else if (ang2 < ang1 && ((ang > 0 && ang < ang2) || (ang > ang1 && ang < 2 * pi))) {
					sumX += xHaarResponses.get(k).floatValue();
					sumY += yHaarResponses.get(k).floatValue();
				}
			}
		    // if the vector produced from this window is longer than all
		    // previous vectors then this forms the new dominant direction
		    if (sumX * sumX + sumY * sumY > max) {
		      // store largest orientation
		      max = sumX * sumX + sumY * sumY;
		      orientation = getAngle(sumX, sumY);
		    }
		}
		// assign orientation of the dominant response vector
		point.setOrientation(orientation);
	}
	
	//! Gets the modified descriptor. See Agrawal ECCV 08
	//! Modified descriptor contributed by Pablo Fernandez
	private void getMDescriptor(InterestPoint point, boolean upright) {
		int y, x, sample_x, sample_y, count = 0;
		int i = 0, ix = 0, j = 0, jx = 0, xs = 0, ys = 0;
		float scale, dx, dy, mdx, mdy, co = 1f, si = 0f;
		float desc[] = new float[64];
		float gauss_s1 = 0.0f, gauss_s2 = 0.0f;
		float rx = 0.0f, ry = 0.0f, rrx = 0.0f, rry = 0.0f, len = 0.0f;
		
		float cx = -0.5f, cy = 0.0f; //Subregion centers for the 4x4 gaussian weighting
		
		scale = point.getScale();
		x = Math.round(point.getX());
		y = Math.round(point.getY());
		if (!upright) {
			co = (float)Math.cos(point.getOrientation());
			si = (float)Math.sin(point.getOrientation());
		}
		i = -8;
		// Calculate descriptor for this interest point
		// Area of size 24 s x 24 s
		while (i < 12) {
			j = -8;
			i = i - 4;
			
			cx += 1.0f;
			cy = -0.5f;
				  
		  	while (j < 12){
				dx = dy = mdx = mdy = 0.0F;
				cy += 1.0f;
				
				j = j - 4;
				
				ix = i + 5;
				jx = j + 5;
				
				xs = Math.round(x + (-jx * scale * si + ix * scale * co));
				ys = Math.round(y + (jx * scale * co + ix * scale * si));
						  
				for (int k = i; k < i + 9; ++k) {
					for (int l = j; l < j + 9; ++l) {
						// Get coords of sample point on the rotated axis
						sample_x = Math.round(x + (-1 * l * scale * si + k * scale * co));
						sample_y = Math.round(y + (     l * scale * co + k * scale * si));
						
						// Get the gaussian weighted x and y responses
						gauss_s1 = gaussian(xs - sample_x, ys - sample_y, 2.5F * scale);
						
						rx = haarX(sample_y, sample_x, 2 * Math.round(scale));
						ry = haarY(sample_y, sample_x, 2 * Math.round(scale));
						
						// Get the gaussian weighted x and y responses on rotated axis
						rrx = gauss_s1 * (-rx * si + ry * co);
						rry = gauss_s1 * (rx * co + ry * si);
						
						dx += rrx;
						dy += rry;
						
						mdx += Math.abs(rrx);
						mdy += Math.abs(rry);
					}
				}
						  
				// Add the values to the descriptor vector
				gauss_s2 = gaussian(cx - 2.0f, cy - 2.0f, 1.5f);
				
				desc[count++] = dx * gauss_s2;
				desc[count++] = dy * gauss_s2; 
				desc[count++] = mdx * gauss_s2;
				desc[count++] = mdy * gauss_s2;
				
				//Accumulate length for vector normalisation
				len += (dx * dx + dy * dy + mdx * mdx + mdy * mdy) * (gauss_s2 * gauss_s2);
				
				j += 9;
			}
			i += 9;
		}
		
		//Convert to Unit Vector
		len = (float)Math.sqrt(len);
		for (i = 0; i < 64; i++) {
			desc[i] /= len;
		}
		
		point.setDescriptor(desc);
	}
	
	//! Calculates Haar wavelet responses in x direction
	//TODO: Why the multiply by 1?
	private float haarX(int row, int column, int s) {
		return SurfUtil.BoxIntegral(mIntegralImage, row - s / 2, column, s, s / 2)
			- 1 * SurfUtil.BoxIntegral(mIntegralImage, row - s / 2, column - s / 2, s, s / 2);
	}

	//! Calculates Haar wavelet responses in y direction
	private float haarY(int row, int column, int s) {
		return SurfUtil.BoxIntegral(mIntegralImage, row, column - s / 2, s / 2, s)
			- 1 * SurfUtil.BoxIntegral(mIntegralImage, row - s / 2, column - s / 2, s / 2, s);
	}
	
	//! Gets the angle from the +ve x-axis of the vector given by (xHaarResponse, yHaarResponse)
	private float getAngle(float xHaarResponse, float yHaarResponse) {
		float pi = SurfUtil.PI;
		if(xHaarResponse >= 0 && yHaarResponse >= 0)
			return (float)Math.atan(yHaarResponse / xHaarResponse);

		if(xHaarResponse < 0 && yHaarResponse >= 0)
			return pi - (float)Math.atan(-yHaarResponse / xHaarResponse);

		if(xHaarResponse < 0 && yHaarResponse < 0)
			return pi + (float)Math.atan(yHaarResponse / xHaarResponse);

		if(xHaarResponse >= 0 && yHaarResponse < 0)
			return 2 * pi - (float)Math.atan(-yHaarResponse / xHaarResponse);

		return 0;
	}
	
	//! Calculates the value of the 2d gaussian at x,y
	private float gaussian(int x, int y, float sig) {
		return (1.0f / (2.0f * SurfUtil.PI * sig * sig)) * (float)Math.exp(-(x * x + y * y) / (2.0f * sig * sig));
	}
	
	//! Calculates the value of the 2d gaussian at x,y
	private float gaussian(float x, float y, float sig) {
		return (1.0f / (2.0f * SurfUtil.PI * sig * sig)) * (float)Math.exp(-(x * x + y * y) / (2.0f * sig * sig));
	}
}